#include"Arduino.h"  

class Wheel
{
  public:
    float k;
    int port_pwm;
    int port_direction;
    unsigned char v_pwm;
    unsigned char v_dir;
    Wheel(int p_pwm,int p_dir,float v_k)
    {
      k=v_k;
      port_pwm=p_pwm;
      port_direction=p_dir;
      pinMode(port_pwm,OUTPUT);
      pinMode(port_direction,OUTPUT); 
      v_pwm=0;
      v_dir=0;
      UpdateSpeed();
    }
    void SetSpeed(int v)
    {
      int temp;
      temp=(int)(k*v+0.5);
      if(temp<0)
      {
        v_dir=1;
        if(temp<-255)
        {
          v_pwm=0;
        }
        else
        {
          v_pwm=255+temp;
        }
      }
      else
      {
        v_dir=0;
        if(temp>255)
        {
          v_pwm=255;
        }
        else
        {
          v_pwm=temp;
        }
      }
    }
    void UpdateSpeed()
    {
      analogWrite(port_pwm,v_pwm);
      digitalWrite(port_direction,v_dir);
    }
};

Wheel 
  LA(6,7,1.0),
  LB(9,8,1.0),
  RA(10,12,-1.0),
  RB(11,13,-1.0);

void SetVelocity(float vel_x,float vel_y,float vel_z)
{
  vel_x -= 64;
  vel_y -= 64;
  vel_z -= 64;
  vel_x *= -3.9;
  vel_y *= 3.9;
  vel_z *= 3.9;
  LA.SetSpeed(vel_y+vel_x-vel_z);
  LB.SetSpeed(vel_y-vel_x-vel_z);
  RA.SetSpeed(vel_y-vel_x+vel_z);
  RB.SetSpeed(vel_y+vel_x+vel_z);
  LA.UpdateSpeed();
  LB.UpdateSpeed();
  RA.UpdateSpeed();
  RB.UpdateSpeed();
}
